#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
#include <stdlib.h>


class TeleopArdrone
{
public:
  TeleopArdrone(); //default constructor
  void mapController();

private:
  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  
  ros::NodeHandle nh_;
  geometry_msgs::Twist cmd;
  std_msgs::Empty empty_;

  //int linear_x, linear_y, linear_z, angular_;
  ros::Publisher pub;
  ros::Publisher land_pub;
  ros::Publisher reset_pub;
  ros::Publisher takeoff_pub;
  ros::Publisher toggleCam_pub;
  ros::Subscriber joy_sub_;
  double buttonTime;
  
  //mapping variables, set up in mapController();
  std::string id;
  long int land;
  long int reset;
  long int takeoff;
  long int togglecam;
  long int deadman_sw;
  float yaw;
  float altitude;
  float alt_up;
  float alt_down;
  float roll;
  float pitch;
  float pitch1;
  float deadzone;
  float alt_deadzone;
};



////////////////////////////////////////////////////////////////
// input: is the SPECIFIC device input (i.e. "mouse1", "js0", etc.)
//        the input is case sensitive.
//
// id1/id2: is the device ID you are looking for. It is case sensitive
// 		suitable ex: "Bus", "Vendor", "Product", "Version", "Name"
//
///////////////////////////////////////////////////////////////

std::string getDeviceId (std::string input, std::string id1, std::string id2) {
  std::string vendID;
  std::string prodID;
  std::ifstream infile;			// file stream
  std::vector<std::string> inLines;	// vector of strings, where each string is a line of the file

  infile.open ("/proc/bus/input/devices", std::ifstream::in);	// file being opened

  while (infile.good()){	// store file lines into vector "inLines"
  	std::string strTemp;			// temp var which stores line
  	getline(infile, strTemp);	// gets file line and stores in strTemp
    inLines.push_back(strTemp);	// pushes file line onto the back of vector
  }
  
  bool js0Found = false;	// "js0" not found, could be anything in input (i.e. /dev/input/xxx)
  bool devIDFound = false;	// device id line found, contains bus, vendor, product, version
  std::string devStr;			// storage for device id line
  
  while(!inLines.empty() && (!devIDFound || !js0Found)){	// searches lines in "inLines"
  	std::string temp = inLines.back();	// searches bottom to top, get last line
  	inLines.pop_back();				// remove last line from "inLines"
  	
  	std::istringstream ins;				// in stringstream for reading string out of the line
	ins.str(temp);					// specify which string
	while(ins.good()){				// keep extracting strings until empty/fail
		std::string word;				// stores word extracted from string
		ins >> word;				// extract word
		if (word == input)			// checks if word extracted == "js0"
			js0Found = true;
		if (word == "I:"){			// checks if device id line found
			devIDFound = true;
			devStr = temp;			// stores device id line to devStr
		}	
	}
  }
  
  // parse device ID line
  std::istringstream ins;		// in stringsteam
  ins.str(devStr);			// setup stringstream
  std::vector<std::string> devID;		// extracts strings between delimiter (spaces in this case)
  std::string vend = id1 + "=";	// Vendor ID we are looking for
  std::string prod = id2 + "=";	// Product ID we are looking for
  
  while(ins.good()){		// store "words" in vector
  	std::string word;			// storage for extracted word
  	ins >> word;
  	devID.push_back(word);	// push word onto devID
  }
  
  while(!devID.empty()){	// searches for strings of interest, Vendor and Product
    std::string temp = devID.back();		// stores string on back of devID
  	if(temp.find(vend) != std::string::npos){	// tests if the string contains "Vendor="
  		temp.erase(temp.begin(), temp.begin()+vend.size());	// erases "Vendor="
  		//cout << temp << endl;
  		vendID = temp;	// store ID in vendID
  	}
  	if(temp.find(prod) != std::string::npos){	// test if string contains "Product="
  		temp.erase(temp.begin(), temp.begin()+prod.size());	// erases "Product="
  		//cout << temp << endl;
  		prodID = temp;	// store ID in prodID
  	}
  	devID.pop_back();	// pops string off devID
  }
  
  infile.close();		// close file

  return vendID + prodID;
}

